#include "gsl/gsl_matrix.h"

#include "math.h"
#include "gsl/gsl_permutation.h"
#include "gsl/gsl_linalg.h"
#include "PowerFlowCalculation.h"

void gsl_matrix_mul(gsl_matrix *a,gsl_matrix *b,gsl_matrix *c)
{
	for (size_t i=0;i<a->size1;i++)
	{
		for (size_t j=0;j<b->size2;j++)
		{
			double sum=0.0;
			for (size_t k=0;k<b->size1;k++)
			{
				sum+=gsl_matrix_get(a,i,k)*gsl_matrix_get(b,k,j);
			}
			gsl_matrix_set(c,i,j,sum);
		}
	}
}

gsl_matrix* gsl_matrix_add2(gsl_matrix *a,gsl_matrix *b)
{
    gsl_matrix* sum = gsl_matrix_alloc(a->size1,a->size2);
    for(int i=0;i<a->size1;i++)
    {
        for(int j=0;j<a->size2;j++)
        {
            gsl_matrix_set(sum,i,j,gsl_matrix_get(a,i,j)+gsl_matrix_get(b,i,j));
        }
    }
    return sum;
}

gsl_matrix* gsl_matrix_sub2(gsl_matrix *a,gsl_matrix *b)
{
    gsl_matrix* c = gsl_matrix_alloc(a->size1,a->size2);
    for(int i=0;i<a->size1;i++)
    {
        for(int j=0;j<a->size2;j++)
        {
            gsl_matrix_set(c,i,j,gsl_matrix_get(a,i,j)-gsl_matrix_get(b,i,j));
        }
    }
    return c;
}

gsl_matrix* gsl_matrix_scale2(gsl_matrix *a,double x)
{
    gsl_matrix* b= gsl_matrix_alloc(a->size1,a->size2);
    for(int i=0;i<a->size1;i++)
    {
        for(int j=0;j<a->size2;j++)
        {
            gsl_matrix_set(b,i,j,x*gsl_matrix_get(a,i,j));
        }
    }
    return b;
}



gsl_matrix* gsl_matrix_mul(gsl_matrix *a,gsl_matrix *b)
{
    gsl_matrix *c = gsl_matrix_alloc(a->size1,b->size2);
	for (size_t i=0;i<a->size1;i++)
	{
		for (size_t j=0;j<b->size2;j++)
		{
			double sum=0.0;
			for (size_t k=0;k<b->size1;k++)
			{
				sum+=gsl_matrix_get(a,i,k)*gsl_matrix_get(b,k,j);
			}
			gsl_matrix_set(c,i,j,sum);
		}
	}
    return c;
}

gsl_matrix* GetInverse(gsl_matrix *A)
{
	int n = A->size1;
	gsl_matrix *tmpA = gsl_matrix_alloc(n, n);
	gsl_matrix_memcpy(tmpA, A);
	gsl_permutation *p = gsl_permutation_alloc(n);
	int sign = 0;
	gsl_linalg_LU_decomp(tmpA, p, &sign);
    
	gsl_matrix* inverse = gsl_matrix_alloc(n, n);
	gsl_linalg_LU_invert(tmpA, p, inverse);
    //
	gsl_permutation_free(p);
	gsl_matrix_free(tmpA);
    //PRINT_MATRIX(inverse);
    return inverse;
}

Power_Calculation_Node Power_Calculation(gsl_matrix* G,gsl_matrix *B,gsl_matrix *PQ_Node,gsl_matrix *PV_Node,gsl_matrix *ESS_Node,gsl_matrix *Swing_Node,int Node_Num,Node m_Node[])
{
    Power_Calculation_Node pcn;
    double PLoss = 0.0;
    double QLoss = 0.0;
    double P_slack = 0.0;
    double Error = 0.01;
    double Current_Error = 100.0;
    int nCount = 0;
    bool Convergence = false;
    int k = 1;

    gsl_matrix *delt_angle = gsl_matrix_alloc(Node_Num-Swing_Node->size1,1);
    gsl_matrix *DeltV_Div_V = gsl_matrix_alloc(PQ_Node->size1,1);
    gsl_matrix *X = gsl_matrix_alloc(delt_angle->size1+DeltV_Div_V->size1,1);
    for(int i=0;i<delt_angle->size1;i++){
        gsl_matrix_set(X,i,0,gsl_matrix_get(delt_angle,i,0));
    }
    for(int i=0;i<DeltV_Div_V->size1;i++){
        gsl_matrix_set(X,delt_angle->size1+i,0,gsl_matrix_get(DeltV_Div_V,i,0));
    }

    gsl_matrix *Input_P = gsl_matrix_alloc(PQ_Node->size1+PV_Node->size1+ESS_Node->size1,1);
    for(int i=0;i<PQ_Node->size1;i++)
    {
        gsl_matrix_set(Input_P,i,0,-gsl_matrix_get(PQ_Node,i,1));
    }
    for(int i=0;i<PV_Node->size1;i++)
    {
        gsl_matrix_set(Input_P,i+PQ_Node->size1,0,gsl_matrix_get(PV_Node,i,1)-gsl_matrix_get(PV_Node,i,5));
    }
    for(int i=0;i<ESS_Node->size1;i++)
    {
        gsl_matrix_set(Input_P,i+ESS_Node->size1+PQ_Node->size1,0,gsl_matrix_get(ESS_Node,i,1)-gsl_matrix_get(ESS_Node,i,5));
    }
    
    gsl_matrix *Input_Q = gsl_matrix_alloc(PQ_Node->size1,1);
    for(int i=0;i<PQ_Node->size1;i++)
    {
        gsl_matrix_set(Input_Q,i,0,-gsl_matrix_get(PQ_Node,i,2));
    }


    int PQ_Num = PQ_Node->size1;
    int PV_Num = PV_Node->size1;
    int ESS_Num = ESS_Node->size1;

    gsl_matrix *voltage = gsl_matrix_alloc(1, PQ_Num + PV_Num + ESS_Num + 1);
    gsl_matrix *angle = gsl_matrix_alloc(1, PQ_Num + PV_Num + ESS_Num + 1);

    for(int i=0;i<PQ_Num;i++)
    {
        gsl_matrix_set(voltage,0,i,gsl_matrix_get(PQ_Node,i,3));
        gsl_matrix_set(angle,0,i,gsl_matrix_get(PQ_Node,i,4));
    }
    for(int i=0;i<PV_Num;i++)
    {
        gsl_matrix_set(voltage,0,i+PQ_Num,gsl_matrix_get(PV_Node,i,2));
        gsl_matrix_set(angle,0,i+PQ_Num,gsl_matrix_get(PV_Node,i,3));
    }
    for(int i=0;i<ESS_Num;i++)
    {
        gsl_matrix_set(voltage,0,i+PQ_Num+PV_Num,gsl_matrix_get(ESS_Node,i,2));
        gsl_matrix_set(angle,0,i+PQ_Num+PV_Num,gsl_matrix_get(ESS_Node,i,3));
    }

    gsl_matrix_set(voltage,0,ESS_Num+PQ_Num+PV_Num,1.06);
    gsl_matrix_set(angle,0,ESS_Num+PQ_Num+PV_Num,gsl_matrix_get(Swing_Node,0,2));

    int P_Num = Node_Num - Swing_Node->size1;
    gsl_matrix *Jocabbi = gsl_matrix_alloc( P_Num+PQ_Num, P_Num+PQ_Num);


    gsl_matrix *Y = gsl_matrix_alloc( P_Num+PQ_Num, 1);
    gsl_matrix_free(X);
    X = gsl_matrix_alloc( P_Num+PQ_Num, 1);
    gsl_matrix *Jocabbi_inv = gsl_matrix_alloc( P_Num+PQ_Num, P_Num+PQ_Num);
    gsl_matrix *delt_angle_tr = gsl_matrix_alloc(1,P_Num);
    gsl_matrix_free(DeltV_Div_V);
    DeltV_Div_V = gsl_matrix_alloc( PQ_Num,1);
    gsl_matrix *voltagee = gsl_matrix_alloc(PQ_Num,1);
    gsl_matrix *voltagee_part = gsl_matrix_alloc( 1,PQ_Num);
    gsl_matrix *DeltV = gsl_matrix_alloc( PQ_Node->size1,1);
    gsl_matrix *DeltV_tr = gsl_matrix_alloc( 1,PQ_Num);
    gsl_matrix *Q = gsl_matrix_alloc( PV_Num+1,1);


    while(Current_Error > Error)
    {
        gsl_matrix *H = gsl_matrix_alloc( P_Num, P_Num);
        gsl_matrix *N = gsl_matrix_alloc( P_Num, PQ_Num);
        gsl_matrix *J = gsl_matrix_alloc( PQ_Num, P_Num);
        gsl_matrix *L = gsl_matrix_alloc( PQ_Num, PQ_Num);
        gsl_matrix *delt_P = gsl_matrix_alloc( P_Num, 1);
        gsl_matrix *delt_Q = gsl_matrix_alloc( PQ_Num, 1);
        
        nCount++;
        //gsl_matrix *H = gsl_matrix_alloc( P_Num, P_Num);
        //PRINT_MATRIX(voltage);
        for(int i=0;i<P_Num;i++)
        {
            for(int j=0;j<P_Num;j++)
            {
                if(i!=j)
                {
                    gsl_matrix_set(H,i,j,-gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,j)*(gsl_matrix_get(G,i,j)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))-gsl_matrix_get(B,i,j)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))));
                }
                else
                {
                    for(int m=0;m<Node_Num;m++)
                    {
                        if(m!=i)
                        {
                            gsl_matrix_set(H,i,i,gsl_matrix_get(H,i,i)+gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,m)*(gsl_matrix_get(G,i,m)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,m))-gsl_matrix_get(B,i,m)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,m))));
                        }
                    }
                }
            }
        }
        //PRINT_MATRIX(H)

        //gsl_matrix *N = gsl_matrix_alloc( P_Num, PQ_Num);
        for(int i=0;i<P_Num;i++)
        {
            for(int j=0;j<PQ_Num;j++)
            {
                if(i!=j)
                {
                    gsl_matrix_set(N,i,j,-gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,j)*(gsl_matrix_get(G,i,j)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))+gsl_matrix_get(B,i,j)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))));
                }
                else
                {
                    for(int m=0;m<Node_Num;m++)
                    {
                        if(m!=i)
                        {
                            gsl_matrix_set(N,i,i,gsl_matrix_get(N,i,i)-gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,m)*(gsl_matrix_get(G,i,m)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,m))+gsl_matrix_get(B,i,m)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,m))));
                        }
                    }
                    gsl_matrix_set(N,i,i,gsl_matrix_get(N,i,i)-2*gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,i)*gsl_matrix_get(G,i,i));
                }
            }
        }
        //gsl_matrix *J = gsl_matrix_alloc( PQ_Num, P_Num);

    
        for(int i=0;i<PQ_Num;i++)
        {
            for(int j=0;j<P_Num;j++)
            {
                if(i!=j)
                {
                    gsl_matrix_set(J,i,j,gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,j)*(gsl_matrix_get(G,i,j)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))+gsl_matrix_get(B,i,j)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))));
                }
                else
                {
                    for(int m=0;m<Node_Num;m++)
                    {
                        if(m!=i)
                        {
                            gsl_matrix_set(J,i,i,gsl_matrix_get(J,i,i)-gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,m)*(gsl_matrix_get(G,i,m)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,m))+gsl_matrix_get(B,i,m)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,m))));
                        }
                    }
                }
            }
        }
        
        // gsl_matrix *L = gsl_matrix_alloc( PQ_Num, PQ_Num);
        for(int i=0;i<PQ_Num;i++)
        {
            for(int j=0;j<PQ_Num;j++)
            {
                if(i!=j)
                {
                    gsl_matrix_set(L,i,j,-gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,j)*(gsl_matrix_get(G,i,j)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))-gsl_matrix_get(B,i,j)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))));
                }
                else
                {
                    for(int m=0;m<Node_Num;m++)
                    {
                        if(m!=i)
                        {
                            gsl_matrix_set(L,i,i,gsl_matrix_get(L,i,i)-gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,m)*(gsl_matrix_get(G,i,m)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,m))-gsl_matrix_get(B,i,m)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,m))));
                        }
                    }
                    gsl_matrix_set(L,i,i,gsl_matrix_get(L,i,i)+2*gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,i)*gsl_matrix_get(B,i,i));
                }
            }
        }

        for(int i=0;i<P_Num;i++)
        {
            for(int j=0;j<P_Num;j++)
            {
                gsl_matrix_set(Jocabbi,i,j,gsl_matrix_get(H,i,j));
            }
        }
        for(int i=0;i<P_Num;i++)
        {
            for(int j=0;j<PQ_Num;j++)
            {
                gsl_matrix_set(Jocabbi,i,j+P_Num,gsl_matrix_get(N,i,j));
            }
        }
        for(int i=0;i<PQ_Num;i++)
        {
            for(int j=0;j<P_Num;j++)
            {
                gsl_matrix_set(Jocabbi,i+P_Num,j,gsl_matrix_get(J,i,j));
            }
        }
        for(int i=0;i<PQ_Num;i++)
        {
            for(int j=0;j<PQ_Num;j++)
            {
                gsl_matrix_set(Jocabbi,i+P_Num,j+P_Num,gsl_matrix_get(L,i,j));
            }
        }


        

        for(int i=0;i<P_Num;i++)
        {
            gsl_matrix_set(delt_P,i,0,gsl_matrix_get(Input_P,i,0));
            for(int j=0;j<Node_Num;j++)
            {
                gsl_matrix_set(delt_P,i,0,gsl_matrix_get(delt_P,i,0)-gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,j)*(gsl_matrix_get(G,i,j)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))+gsl_matrix_get(B,i,j)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))));
            }
        }

        

        for(int i=0;i<PQ_Num;i++)
        {
            gsl_matrix_set(delt_Q,i,0,gsl_matrix_get(Input_Q,i,0));
            for(int j=0;j<Node_Num;j++)
            {
                gsl_matrix_set(delt_Q,i,0,gsl_matrix_get(delt_Q,i,0)-gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,j)*(gsl_matrix_get(G,i,j)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))-gsl_matrix_get(B,i,j)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))));
            }
        }        
//         PRINT_MATRIX(delt_P)
//         printf("\n");
//         PRINT_MATRIX(delt_Q);


// printf("\n");

// printf("\n");
// printf("\n");
// return pcn;
        double err = abs(gsl_matrix_max(delt_Q));
        Current_Error = abs(gsl_matrix_max(delt_P));
        //PRINT_MATRIX(delt_Q)
        

        if (Current_Error <  err)
        {
            Current_Error = err;
        }
        // printf("current error %lf\n",Current_Error);
        // return pcn;
        for(int i=0;i<P_Num;i++)
        {
            gsl_matrix_set(Y,i,0,gsl_matrix_get(delt_P,i,0));
        }
        for(int i=0;i<PQ_Num;i++)
        {
            gsl_matrix_set(Y,i+P_Num,0,gsl_matrix_get(delt_Q,i,0));
        }
   
        Jocabbi_inv=GetInverse(Jocabbi);  
        
        gsl_matrix_mul(Jocabbi_inv,Y,X); 

        for(int i=0;i<P_Num;i++)
        {
            gsl_matrix_set(delt_angle,i,0,gsl_matrix_get(X,i,0));
        }
        

        gsl_matrix_transpose_memcpy(delt_angle_tr,delt_angle);
  
        //printf("angle size1:%d\n",angle->size1);
        for(int i=0;i<P_Num;i++)
        {
            //gsl_matrix_set(angle,0,i,0);
            gsl_matrix_set(angle,0,i,gsl_matrix_get(angle,0,i)-gsl_matrix_get(delt_angle_tr,0,i));
        }

        for(int i=0;i<PQ_Num;i++)
        {
            gsl_matrix_set(DeltV_Div_V,i,0,gsl_matrix_get(X,P_Num+i,0));
        }

        for(int i=0;i<PQ_Num;i++)
        {
            gsl_matrix_set(voltagee_part,0,i,gsl_matrix_get(voltage,0,i));
        }
         
        gsl_matrix_transpose_memcpy(voltagee,voltagee_part);
     
        gsl_matrix_memcpy(DeltV,DeltV_Div_V);
        
        gsl_matrix_mul_elements(DeltV,voltagee);

        gsl_matrix_transpose_memcpy(DeltV_tr,DeltV); 
        
        for(int i=0;i<PQ_Num;i++)
        {
            gsl_matrix_set(voltage,0,i,gsl_matrix_get(voltage,0,i)-gsl_matrix_get(DeltV_tr,0,i));
        }
        //printf("count=%d current error=%.20lf\n",nCount,Current_Error);
        
        //printf("\n");

        //return pcn;

            
        gsl_matrix_free(H);
        gsl_matrix_free(L);
        gsl_matrix_free(N);
        gsl_matrix_free(J);
        gsl_matrix_free(delt_Q);
        gsl_matrix_free(delt_P);
    }
    
   //return pcn;  
    if ( Current_Error < Error )
    {
        Convergence = true;
        // for(int i=0;i<Node_Num;i++)
        // {
        //     gsl_matrix_set(voltagee,i,0,gsl_matrix_get(voltage,0,i));
        // }
        PLoss = 0.0;
        QLoss = 0.0;
        for(int i=0;i<Node_Num;i++)
        {
            for(int j=0;j<Node_Num;j++)
            {
                if(j!=i)
                {
                    PLoss = PLoss+gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,j)*(gsl_matrix_get(G,i,j)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j)));
                    
                }
                QLoss = QLoss+gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,j)*(gsl_matrix_get(G,i,j)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))-gsl_matrix_get(B,i,j)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j)));
            
            }
            //return pcn;

            PLoss = PLoss+gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,i)*gsl_matrix_get(G,i,i);
            
        }
        k=0;
        
        for(int i=0;i<Node_Num;i++)
        {
            if(m_Node[i].Type == 1)
            {

                for(int j=0;j<Node_Num;j++)
                {
                    gsl_matrix_set(Q,k,0,gsl_matrix_get(Q,k,0)+gsl_matrix_get(voltage,0,i)*gsl_matrix_get(voltage,0,j)*(gsl_matrix_get(G,i,j)*sin(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))-gsl_matrix_get(B,i,j)*cos(gsl_matrix_get(angle,0,i)-gsl_matrix_get(angle,0,j))));
                }
                //printf("i=%d\n",i);
                gsl_matrix_set(Q,k,0,gsl_matrix_get(Q,k,0)+gsl_matrix_get(PV_Node,k,6));
                //return pcn;
                k++;
            }
        }
        P_slack = 0.0;
        for(int i=0;i<Node_Num;i++)
        {
            double f = gsl_matrix_get(angle,0,Node_Num-1) - gsl_matrix_get(angle,0,i);
            P_slack = P_slack + gsl_matrix_get(voltage,0,Node_Num-1)*gsl_matrix_get(voltage,0,i)*(gsl_matrix_get(G,Node_Num-1,i)*cos(f)+gsl_matrix_get(B,Node_Num-1,i)*sin(f));
        }
        P_slack = P_slack + gsl_matrix_get(Swing_Node,0,5);
    }

    pcn.PLoss = PLoss;
    pcn.voltagee =voltagee;
    pcn.angle = angle;
    return pcn;


}